from matplotlib import colors
import matplotlib.pyplot as plt
import imageio

def vis_corner(corner,pos_grid, vis_cor = True,vis_line = True):
    '''
    file    :visualiz_f.py
    descri  :
    time    :2021/08/13 15:23:25
    author :binyao
    version :1.0
    input   : 
            corner: 分割出来的转弯角在格点化数据 pos_grid 中的索引
    output  :
            绘制转弯角和中间的直线段
    '''
    fig = plt.figure(figsize=(16, 16))
    plt.ion()
    image_list = []
    # for i in range(10, corner.shape[0] - 1):
    for i in range(len(corner)-1):
        if vis_cor:
            start, stop = corner[i,0], corner[i,1]
            plt.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
            plt.show()
            plt.pause(1)
        if vis_line:
            start, stop = corner[i,1], corner[i+1,0]
            plt.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
            plt.show()
            plt.pause(0.5)
            plt.savefig('temp.png')
            image_list.append(imageio.imread('temp.png'))
    imageio.mimsave('pic.gif', image_list, duration=1)
        # if (i % 8) ==0:
        #     plt.clf()
    # print(corner)
    # print(len(corner))
    # fig = plt.figure(figsize=(1, 2))
    # plt.ion()
    # for i in range(10, corner.shape[0] - 1):
    #     ax = fig.add_subplot(1, 2, 1)
    #     start, stop = corner[i,0], corner[i,1]
    #     ax.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
    #     ax = fig.add_subplot(1, 2, 2)
    #     start, stop = corner[i,1], corner[i+1,0]
    #     ax.plot(pos_grid[start:stop+1,0],pos_grid[start:stop+1,1], marker=',')
    #     plt.show()
    #     plt.pause(0.5)



    # 画3d的
    # fig = plt.figure()
    # plt.gca()
    # ax = fig.gca(projection='3d')
    # ax.set_title("3D_Curve")
    # ax.set_xlabel("x")
    # ax.set_ylabel("y")
    # ax.set_zlabel("z")   
    # for i in range(data_seg.shape[0]):
    #     start, stop = data_seg[i,0], data_seg[i,1]
    #     ax.plot(pos[start:stop+1,2],pos[start:stop+1,1],pos[start:stop+1,3], marker=',')
    # plt.show()
def visualLoopClosure(pos_grid,sim_loop):
    fig = plt.figure()
    plt.ion()
    image_list = []
    for each in sim_loop:
        s1, s2 = each[0], each[1]
        # fig = plt.figure(figsize=(15, 5))
        ax = fig.add_subplot(1, 2, 1)
        ax.axis('equal')
        ax.plot(pos_grid[:s1[0],0],pos_grid[:s1[0],1], color='black',marker=',',label="reference") #绘制整个的历史轨迹
        ax.plot(pos_grid[s1,0],pos_grid[s1,1], color='red',marker=',',label="s1") #绘制的闭环轨迹段s1
        ax.plot(pos_grid[s2,0],pos_grid[s2,1], color='green',marker=',',label="s2") #绘制的闭环轨迹段s2
        plt.legend(loc="lower left")
        # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
        # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
        # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
        ax = fig.add_subplot(1, 2, 2)
        ax.plot(pos_grid[s1,7], marker=',')
        ax.plot(pos_grid[s2,7], marker=',')
        plt.savefig('temp.png')
        image_list.append(imageio.imread('temp.png'))
        plt.show()
        plt.pause(0.1)
        plt.clf()

    # plt.pause(600)
    imageio.mimsave('pic2.gif', image_list, duration=0.4)


def visualTrajAndMag(pos_grid):

    fig = plt.figure()
    plt.ion()
    image_list = [] 

    indoor_idx = 0 #室内车库开始的位置
    window_slide = 400 #长度为window_slide/10 m的滑窗

    # print(indoor_idx + window_slide, len(pos_grid) - window_slide, int(window_slide/2))
    # input()
    # fig = plt.figure()
    for i in range(indoor_idx + window_slide, len(pos_grid) - window_slide, int(window_slide/4)):
        traj_now = pos_grid[i:i + window_slide,:]
        traj_history = pos_grid[indoor_idx:i,:]
        ax = fig.add_subplot(1, 2, 1)
        ax.plot(traj_history[:,0],traj_history[:,1], color='blue',marker=',',label="reference") #绘制整个的历史轨迹
        ax.plot(traj_now[:,0],traj_now[:,1], color='red',marker=',',label="s1",linestyle='--') #绘制的闭环轨迹段s1
       
        ax.axis('equal')
        plt.legend(loc="lower left")
        
        # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
        # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
        # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
        ax = fig.add_subplot(1, 2, 2)
        # ax.ticklabel_format(style='plain')
        # ax.plot(traj_now[:,4], marker=',',label="x")
        # ax.plot(traj_now[:,5], marker=',',label="y")
        ax.plot(traj_now[::-1,4], marker=',',label="x")
        ax.plot(traj_now[::-1,5], marker=',',label="y")
        ax.plot(traj_now[:,6], marker=',',label="z")
        # ax.plot(traj_now[::-1,6], marker=',',label="z_reverse")
        ax.plot(traj_now[::-1,7], marker=',',label="all")
        plt.legend(loc="lower left")
    
        plt.savefig('./imagesXYBackward/temp'+str(i)+'.png')
        # plt.savefig('temp.png')
        # image_list.append(imageio.imread('temp.png'))
        plt.show()
        plt.pause(0.1)
        plt.clf()
    # imageio.mimsave('Traj#Mag.gif', image_list, duration=0.4)

def visualTrajLineAndMag(pos_grid,corner):
    
    fig = plt.figure()
    plt.ion()
    image_list = []
    for i in range(corner.shape[0]-1):
        print(i)
        # start_s1, stop_s1 = corner[i-1,0], corner[i-1,1]
        # start_s1, stop_s1 = corner[i,0], corner[i,1] #这是转弯
        start_s1, stop_s1 = corner[i,1], corner[i+1,0] #这是直线
        if stop_s1 - start_s1 < 100:
            print('长度不够')
        traj_now = pos_grid[start_s1:stop_s1,:]
        traj_history = pos_grid[:start_s1,:]
        ax = fig.add_subplot(1, 2, 1)
        ax.plot(traj_history[:,0],traj_history[:,1], color='blue',marker=',',label="reference") #绘制整个的历史轨迹
        ax.plot(traj_now[:,0],traj_now[:,1], color='red',marker=',',label="s1",linestyle='--') #绘制的闭环轨迹段s1
       
        ax.axis('equal')
        plt.legend(loc="lower left")
        
        # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
        # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
        # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
        ax = fig.add_subplot(1, 2, 2)
        # ax.ticklabel_format(style='plain')
        ax.plot(traj_now[:,4], marker=',',label="x")
        ax.plot(traj_now[:,5], marker=',',label="y")
        ax.plot(traj_now[:,6], marker=',',label="z")
        ax.plot(traj_now[:,7], marker=',',label="all")
        plt.legend(loc="lower left")
    

        plt.savefig('./images/temp'+str(i)+'.png')
        image_list.append(imageio.imread('temp.png'))
        plt.show()
        plt.pause(0.4)
        plt.clf()
    imageio.mimsave('TrajLine#Mag.gif', image_list, duration=1)

def visualLoopClosureOnce(pos_grid, sim_loop, fig):
    '''
    file    :visualiz_f.py
    descri  :单次显示的
    time    :2021/11/11 20:48:37
    author :binyao
    version :1.0
    input   :
    output  :
    '''
    # for each in sim_loop:
    # print(sim_loop.shape[0])
    # for each in sim_loop:
    #     print(each)
    s1, s2 = sim_loop[0], sim_loop[1]
    # fig = plt.figure(figsize=(15, 5))
    ax = fig.add_subplot(1, 2, 1)
    ax.axis('equal')
    ax.plot(pos_grid[:s1[0],1],pos_grid[:s1[0],0], color='pink',marker=',',label="reference") #绘制整个的历史轨迹
    
    ax.plot(pos_grid[s2,1] , pos_grid[s2,0] , color='green',marker=',',label="s2") #绘制的闭环轨迹段s2
    ax.plot(pos_grid[s1,1],pos_grid[s1,0], color='red',marker=',',label="s1") #绘制的闭环轨迹段s1
    plt.legend(loc="lower left")
    # ax.plot(pos_grid[4000:s1[-1],0],pos_grid[4000:s1[-1],1], marker=',',label="reference")
    # ax.plot(pos_grid[s1,0],pos_grid[s1,1], marker=',',label="s1")
    # ax.plot(pos_grid[s2,0],pos_grid[s2,1], marker=',',label="s2")
    ax = fig.add_subplot(1, 2, 2)
    ax.plot(pos_grid[s1,7], marker=',')
    ax.plot(pos_grid[s2,7], marker=',')

    plt.show()
    plt.pause(0.1)
    # plt.clf()

    # plt.pause(600)
    # imageio.mimsave('pic2.gif', image_list, duration=1)